/*
 * @Author: LVGRAPE
 * @LastEditors: LVGRAPE
 */
#include "at32f421.h"
void servo_gpio_init(void)
{
    gpio_init_type gpio_init_struct;

    /* gpioa gpiob clock enable */
    crm_periph_clock_enable(CRM_GPIOB_PERIPH_CLOCK, TRUE);

    gpio_default_para_init(&gpio_init_struct);

    gpio_init_struct.gpio_pins = GPIO_PINS_14 | GPIO_PINS_15;
    gpio_init_struct.gpio_mode = GPIO_MODE_MUX;
    gpio_init_struct.gpio_out_type = GPIO_OUTPUT_PUSH_PULL;
    gpio_init_struct.gpio_pull = GPIO_PULL_NONE;
    gpio_init_struct.gpio_drive_strength = GPIO_DRIVE_STRENGTH_STRONGER;
    gpio_init(GPIOB, &gpio_init_struct);

    gpio_pin_mux_config(GPIOB, GPIO_PINS_SOURCE14, GPIO_MUX_1); // PB14 -> TMR15_CH1,SERVO1
    gpio_pin_mux_config(GPIOB, GPIO_PINS_SOURCE15, GPIO_MUX_1); // PB15 -> TMR15_CH2,SERVO2
}
void servo_tmr_init(void)
{
    uint16_t prescaler_value = 0;
    tmr_output_config_type tmr_oc_init_structure;
    /* tmr16 clock enable */
    crm_periph_clock_enable(CRM_TMR15_PERIPH_CLOCK, TRUE);

    /* compute the prescaler value */
    prescaler_value = (uint16_t)(system_core_clock / 1000000) - 1;

    /* tmr16 time base configuration */
    tmr_base_init(TMR15, 20000, prescaler_value);

    tmr_cnt_dir_set(TMR15, TMR_COUNT_UP);

    tmr_clock_source_div_set(TMR15, TMR_CLOCK_DIV1);

    tmr_output_default_para_init(&tmr_oc_init_structure);
    tmr_oc_init_structure.oc_mode = TMR_OUTPUT_CONTROL_PWM_MODE_B;
    tmr_oc_init_structure.oc_output_state = TRUE;
    tmr_oc_init_structure.oc_polarity = TMR_OUTPUT_ACTIVE_LOW;
    tmr_oc_init_structure.oc_idle_state = TRUE;
    tmr_output_channel_config(TMR15, TMR_SELECT_CHANNEL_1, &tmr_oc_init_structure);
    tmr_output_channel_config(TMR15, TMR_SELECT_CHANNEL_2, &tmr_oc_init_structure);
    tmr_channel_value_set(TMR15, TMR_SELECT_CHANNEL_1, 0);
    tmr_channel_value_set(TMR15, TMR_SELECT_CHANNEL_2, 0);

    tmr_output_channel_buffer_enable(TMR15, TMR_SELECT_CHANNEL_1, TRUE);
    tmr_output_channel_buffer_enable(TMR15, TMR_SELECT_CHANNEL_2, TRUE);

    tmr_period_buffer_enable(TMR15, TRUE);

    tmr_output_enable(TMR15, TRUE);
    tmr_counter_enable(TMR15, TRUE);
}
void servo1_set_angle(uint16_t angle, confirm_state onOff)
{
    uint16_t pulse_width = angle * 2000 / 180 + 500;
    tmr_channel_value_set(TMR15, TMR_SELECT_CHANNEL_1, onOff ? pulse_width : 0);
}
void servo2_set_angle(uint16_t angle, confirm_state onOff)
{
    uint16_t pulse_width = angle * 2000 / 180 + 500;
    tmr_channel_value_set(TMR15, TMR_SELECT_CHANNEL_2, onOff ? pulse_width : 0);
}
